/*
 * ModbusProtocol.cpp
 *
 *  Created on: May 13, 2011
 *      Author: stud
 */

#include "ModbusProtocol.h"
#include <stdio.h>
#include <stdlib.h>

mbPacket * ModbusProtocol::createReadRequest(mbPacket * packet, unsigned char address, unsigned short numWords, unsigned short startAddress)
{
	packet->header.slaveAddress = address;
	packet->header.op = 0x3;

	packet->tail.readPacketRequest.numWord.Word = numWords;
	packet->tail.readPacketRequest.startWord.Word = startAddress;

	return packet;
}
unsigned char * ModbusProtocol::serializeReadRequest(unsigned char * buffer, mbPacket packet)
{
	buffer[0] = packet.header.slaveAddress;
	buffer[1] = packet.header.op;
	buffer[2] = packet.tail.readPacketRequest.startWord.Bytes.ByteHi;
	buffer[3] = packet.tail.readPacketRequest.startWord.Bytes.ByteLo;
	buffer[4] = packet.tail.readPacketRequest.numWord.Bytes.ByteHi;
	buffer[5] = packet.tail.readPacketRequest.numWord.Bytes.ByteLo;
	return buffer;
}

mbPacket * ModbusProtocol::deserializeReadResponse(unsigned char * buffer, mbPacket * packet)
{
	int i = 0;
		int numWords=0;

#ifdef _debug
		printf("\nDeserializing packet");
		fflush(stdout);
#endif

		packet->header.slaveAddress = buffer[0];
		packet->header.op = buffer[1];

		packet->tail.readPacketResponse.byteNum = buffer[2];

		numWords = buffer[2]/2;

#ifdef _debug
		printf("\nReserving memory");
		fflush(stdout);
#endif
		packet->tail.readPacketResponse.data = (sWord*)malloc(sizeof(sWord) * numWords);

#ifdef _debug
		printf("\nMemory reserved");
		fflush(stdout);
#endif

		int shift = 3;
		int shift2 = 0;

		for(i= 0; i< numWords;i++)
		{
			printf(" %d ",buffer[i+shift]);
			printf(" %d ", buffer[i+shift+1]);
			packet->tail.readPacketResponse.data[i].Bytes.ByteHi = buffer[i+shift];
			packet->tail.readPacketResponse.data[i].Bytes.ByteLo = buffer[i+shift+1];

			shift = shift+1;
		}

		packet->tail.readPacketResponse.CRC.Bytes.ByteHi = buffer[numWords*2+3];
		packet->tail.readPacketResponse.CRC.Bytes.ByteLo = buffer[numWords*2+4];


		return packet;
}
void ModbusProtocol::printReadResponse(mbPacket  packet)
{
	int i = 0;

	printf("\nPacket from slave: %d", packet.header.slaveAddress);
	printf("\nOperation code: %d Expected: 0x3", packet.header.op);
	printf("\nByte count: %d. Number of Words %d", packet.tail.readPacketResponse.byteNum, packet.tail.readPacketResponse.byteNum/2);

	printf("\nReceived data: ");

	int shift = 0;
	for(i=0;i<packet.tail.readPacketResponse.byteNum/2;i++)
	{
		printf(" %d ", packet.tail.readPacketResponse.data[i].Bytes.ByteHi);
		printf(" %d ", packet.tail.readPacketResponse.data[i].Bytes.ByteLo);
	}

	printf("\nCRC byte Hi: %d", packet.tail.readPacketResponse.CRC.Bytes.ByteHi);
	printf("\nCRC byte Lo: %d", packet.tail.readPacketResponse.CRC.Bytes.ByteLo);

	printf("\n");
}


unsigned short ModbusProtocol::crc16( unsigned char * pucFrame, unsigned short usLen )
{
    unsigned char ucCRCHi = 0xFF;
    unsigned char           ucCRCLo = 0xFF;
    int             iIndex;

    while( usLen-- )
    {
        iIndex = ucCRCLo ^ *( pucFrame++ );
        ucCRCLo = ( unsigned char )( ucCRCHi ^ aucCRCHi[iIndex] );
        ucCRCHi = aucCRCLo[iIndex];
    }
    return ( unsigned short )( ucCRCHi << 8 | ucCRCLo );
}

sWord * ModbusProtocol::reverseWord(sWord * word)
{
	unsigned char temp = word->Bytes.ByteHi;
	word->Bytes.ByteHi = word->Bytes.ByteLo;
	word->Bytes.ByteLo = temp;

	return word;
}
void ModbusProtocol::addCRC(unsigned char * bufferOut,unsigned int len)
{
	sWord crc;

		crc.Word = crc16(bufferOut,6);
		reverseWord(&crc);

		bufferOut[len] = crc.Bytes.ByteHi;
		bufferOut[len+1] = crc.Bytes.ByteLo;
}


mbPacket * ModbusProtocol::createWriteRequest(mbPacket * packet, unsigned char address, sWord data, unsigned short startWord)
{
	packet->header.slaveAddress = address;
	packet->header.op = 0x6;

	packet->tail.writeSingleRegRequest.data.Word = data.Word;
	packet->tail.writeSingleRegRequest.startWord.Word =  startWord;

	return packet;
}
unsigned char * ModbusProtocol::serializeWriteRequest(unsigned char * buffer, mbPacket packet)
{
	buffer[0] = packet.header.slaveAddress;
	buffer[1] = packet.header.op;

	buffer[2] = packet.tail.writeSingleRegRequest.startWord.Bytes.ByteHi;
	buffer[3] = packet.tail.writeSingleRegRequest.startWord.Bytes.ByteLo;

	buffer[4] = packet.tail.writeSingleRegRequest.data.Bytes.ByteHi;
	buffer[5] = packet.tail.writeSingleRegRequest.data.Bytes.ByteLo;

	return buffer;
}

mbPacket * ModbusProtocol::deserializeWriteResponse(unsigned char * buffer, mbPacket * packet)
{
	packet->header.slaveAddress = buffer[0];
	packet->header.op = buffer[1];

	packet->tail.writeSingleRegResponse.startWord.Bytes.ByteHi = buffer[2];
	packet->tail.writeSingleRegResponse.startWord.Bytes.ByteLo = buffer[3];

	packet->tail.writeSingleRegResponse.data.Bytes.ByteHi = buffer[4];
	packet->tail.writeSingleRegResponse.data.Bytes.ByteLo = buffer[5];

	packet->tail.writeSingleRegResponse.CRC.Bytes.ByteHi = buffer[6];
	packet->tail.writeSingleRegResponse.CRC.Bytes.ByteLo = buffer[7];


	return packet;
}
void ModbusProtocol::printWriteResponse(mbPacket packet)
{
	printf("\nPacket from slave: %d", packet.header.slaveAddress);
		printf("\nOperation code: %d", packet.header.op);
		printf("\nStart Word byte Hi %d", packet.tail.writeSingleRegResponse.startWord.Bytes.ByteHi);
		printf("\nStart Word byte Lo %d", packet.tail.writeSingleRegResponse.startWord.Bytes.ByteLo);
		printf("\nData byte Hi %d", packet.tail.writeSingleRegResponse.data.Bytes.ByteHi);
		printf("\nData byte Lo %d", packet.tail.writeSingleRegResponse.data.Bytes.ByteLo);
		printf("\nCRC Hi: %d CRC Lo: %d", packet.tail.writeSingleRegResponse.CRC.Bytes.ByteHi, packet.tail.writeSingleRegResponse.CRC.Bytes.ByteLo);

}

ModbusProtocol::ModbusProtocol()
{

}
ModbusProtocol::~ModbusProtocol()
{

}
